#pragma once
#include "zuc/nml_intf/zucpos2.h"
#include <memory>
#include <atomic>
#include <list>
#include <mutex>

namespace axisgroup {
class ServoFilterIntf;
class StopPlanner;
}  // namespace axisgroup

namespace kine {
class KineItf;
}

namespace axisgroup {
class RtPlanServo
{
public:
    enum ServoMode
    {
        SERVO_JOINT = 0,
        SERVO_COORD = 1,
    };
    struct ServoData
    {
        bool is_coord;
        AxisGroupPose cmd;
    };

public:
    explicit RtPlanServo(int id);
    virtual ~RtPlanServo() = default;
    int init(std::shared_ptr<kine::KineItf> kine, AxisGroupPose pose, double ts);
    void set_active(bool active);
    void pause(bool en);
    int update(AxisGroupPose desired, AxisGroupPose& target);
    int push_cmd(bool abs, int step_num, const AxisGroupPose& cmd);
    void get_fdb(RobJointVal& pos_fdb, RobJointVal& tor_fdb);
    void set_mode(ServoMode mode) { mode_ = mode; }
    bool is_active() { return active_; }
    bool is_stopped() { return is_stopped_; }
    double get_frq_time() const { return ts_; }
    std::shared_ptr<ServoFilterIntf> servo_filter_;

private:
    std::shared_ptr<axisgroup::StopPlanner> stop_planner_;
    void refine_pose(AxisGroupPose& pData);
    std::shared_ptr<kine::KineItf> kine_;
    std::atomic<bool> active_;                // 是否正在叠加
    std::atomic<bool> paused_;                // 是否正在暂停
    std::atomic<bool> wish_to_stop_ = false;  // 用于第一个周期初始化
    std::atomic<bool> is_stopped_ = false;    // 是否已经停止
    int group_id_ = -1;
    ZucPose tool = {};
    ZucPose user_frame = {};
    double ts_ = 0;        // 调用频率
    RobJointVal cmd_vel_;  // servo 关节速度

    ServoMode mode_ = SERVO_JOINT;
    AxisGroupPose last_cmd_;      // 用户线层调用
    ServoData last_user_cmd_rt_;  // 实时线程调用，未滤波前
    AxisGroupPose last_cmd_rt_;   // 实时线程调用，滤波后，实际发送给servo

    std::mutex cmd_mutex_;
    std::list<ServoData> cmd_list_;
};
}  // namespace axisgroup